from navi_func import *
import time
from map_func import *
from remote_api import *
from pprint import pprint


def task_impl(dst_loc):
        navi = NaviFunction()
        map = MapFunction()
        api = RemoteAPI()
        api.status()
        
        loc = api.get_json_parse()
        robot_loc = map.robot_loc_by_dic(loc)
        shortest_path_list = navi.multi_dikstra_shortest_path(robot_loc, dst_loc, map.graph)
        robot_id = 106
        commands = []
        for path in shortest_path_list:
            for next in shortest_path_list[path]:
                payload = api.post_json_gen(robot_id,map.point_dic[next])
                commands.append(payload)
                api.control(payload)
                time.sleep(0.5)
            robot_id += 1 

def main():
    print("多智能体导航已启动")
    print("请选择任务：")
    print("1. 机器人取货")
    print("2. 机器人送货")

    choice = input("请输入任务编号: ")

    # Switch structure to execute tasks based on user input
    if choice == '1':
        print("执行机器人取货任务")
        # Call function for robot pickup task
        dst_loc = [1235424, 1235413, 1235417, 1235428, 1235326] #这是取货点坐标id

        task_impl(dst_loc)
    elif choice == '2':
        print("执行机器人送货任务")
        # Call function for robot delivery task
        dst_loc = [1235237, 1235416, 1235231, 1235324, 1235347] #这是送货点坐标id


        task_impl(dst_loc)
    else:
        print("无效的任务编号，请重新选择")

if __name__ == "__main__":
    main()